#ifndef __EKF__
#define __EKF__

#include  "common.h"
#include "matching_result.h"
#include <Eigen/Dense>
#include <robot_control/Landmarks.h>
#include <map>
#include <vector>

//TODO(jingjing)take this fromparameter server
#define WB (0.4)

typedef std::vector<Coordinates> Bearings;

class Ekf {
 public:
	 void Predict();
	 void AssociateLandmarks(const robot_control::LandmarksConstPtr &msg,
			 double pose_x, double pose_y,
			 double angle);
	 void Update();
	 void Augment();
	 void CorrectPose(double *x, double *y, double *angle, double velocity,
			 double rotation_speed, double time_elapsed);
	 Ekf();
	 ~Ekf() {};
 private:
	 double x_;
	 double y_;
	 double angle_;
	 double velocity_;
	 double rotation_speed_;
	 double time_elapsed_;
	 Eigen::MatrixXd P_; //covariance for SLAM state (robot pose + feature pose)
	                     //JJ The size maybe is (3+size(feature)*2), how to extend this size everytime?

	 std::vector<Landmark> visible_landmarks_;
	 size_t new_size_;
	 Bearings bearings_new_;
	 Bearings bearings_old_;
	 std::vector<int> bearings_old_id_;

	 double pi_to_pi(const double angle) const;
};

#endif
